#pragma once

#include "armor.hpp"

class Compensator {
 private:
  double ballet_speed_;
  cv::Mat cam_mat_, distor_coff_;
  double gun_cam_distance;  //枪口到镜头的距离
  double distance;            //装甲到镜头的距离
  // cv::Mat rot_vec, trans_vec;  //装甲和镜头之间的旋转矩阵，平移矩阵

  double PinHoleEstimate(Armor& armor);
  double SolveSurfaceLanchAngle(cv::Point2f target);

  void VisualizePnp(Armor& armor, const cv::Mat& output, bool add_lable);

 public:
  Compensator();
  Compensator(const std::string& cam_mat_path);
  ~Compensator();

  // cv::Vec3f EstimateWorldCoord(Armor& armor);
  void SolveAngles(Armor& armor);

  void P4PSolver(Armor& armor);

  void PinHoleSolver(Armor& armor);

  void LoadCameraMat(const std::string& path);

  void Apply(std::vector<Armor>& armors, const cv::Mat& frame);

//void VisualizeResult(std::vector<Armor>& armors, const cv::Mat& output,int verbose = 1);

  void CompensateGravity(Armor& armor);
};
